/*
 * Copyright (c) 2007 T.Cogghe, D.Leroux, S.Lévignac, S.Rives
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */
#include "Matrice.hh"
#include "Astre.hh"
#include <cstring>

///* construit un vecteur ad hoc */
//Vecteur::Vecteur(const Reel x,const Reel y,const Reel z) {
//	lignes[0]=x;
//	lignes[1]=y;
//	lignes[2]=z;
//}
//
///* construit un vecteur à partir d'un astre : (x,y,1) */
//Vecteur::Vecteur(const Astre&a) {
//	lignes[0]=a.x();
//	lignes[1]=a.y();
//	lignes[2]=1.;
//}
//
///* duplique */
//Vecteur::Vecteur(const Vecteur&v) {
//	memcpy(lignes,v.lignes,3*sizeof(Reel));
//}
//
//
///* détruit */
//Vecteur::~Vecteur() {
//}
//
//Vecteur&Vecteur::operator=(const Vecteur&v) {
//	memcpy(lignes,v.lignes,3*sizeof(Reel));
//	return *this;
//}
//
///* accède à un coefficient */
//const Reel& Vecteur::operator()(const int i) const {
//	return lignes[i];
//}
//
//Reel& Vecteur::operator()(const int i) {
//	return lignes[i];
//}
//
//



/* construit une matrice ad hoc */
//Matrice::Matrice(const Reel a,const Reel b,const Reel c,
//		 const Reel d,const Reel e,const Reel f,
//		 const Reel g,const Reel h,const Reel i)
//	: v1(a,b,c),
//	  v2(d,e,f),
//	  v3(g,h,i),
//	  colonnes((Vecteur*[]){&v1,&v2,&v3})
//{}
//
/* construit l'identité */
//Matrice::Matrice() : v1(1.,0.,0.),
//		     v2(0.,1.,0.),
//		     v3(0.,0.,1.),
//		     colonnes((Vecteur*[]){&v1,&v2,&v3})
//{}
//
///* duplique */
//Matrice::Matrice(const Matrice&m) : v1(m.col(0)),
//				   v2(m.col(1)),
//				   v3(m.col(2)),
//				   colonnes((Vecteur*[]){&v1,&v2,&v3})
//{}
//
//
///* construit la matrice à partir de 3 vecteurs colonne */
//Matrice::Matrice(const Vecteur&a,const Vecteur&b,const Vecteur&c)
//	: v1(a),
//	  v2(b),
//	  v3(c),
//	  colonnes((Vecteur*[]){&v1,&v2,&v3})
//{}
//
///* construit la matrice à partir de 3 astres (avec un astre centré sur (x,y), on forme le vecteur colonne (x,y,1) */
//Matrice::Matrice(const Astre&a,const Astre&b,const Astre&c)
//	: v1(a.x(),a.y(),1.),
//	  v2(b.x(),b.y(),1.),
//	  v3(c.x(),c.y(),1.),
//	  colonnes((Vecteur*[]){&v1,&v2,&v3})
//{}
//
///* détruit */
//Matrice::~Matrice()
//{}
//
///* accède à un coefficient */
//const Reel& Matrice::operator() (const int ligne,const int colonne) const {
//	return (*colonnes[colonne])(ligne);
//}
//
//Reel& Matrice::operator() (const int ligne,const int colonne) {
//	return (*colonnes[colonne])(ligne);
//}
//
//
//const class Vecteur&Matrice::col(const int i) const {
//	return *(colonnes[i]);
//}
//
//
//Matrice&Matrice::operator=(const Matrice&m) {
//	v1=m.col(0);
//	v2=m.col(1);
//	v3=m.col(2);
//	return *this;
//}
//
//
/* dit si la matrice est inversible */
bool Matrice::inversible() {
	/* FIXME : check critère */
	return determinant()!=0.;
}






void fadeev(const Matrice&A,Matrice&Ainv) {
	Reel qi=1;
	Matrice Ai,Bi,Bi1;	/* B0 est l'identité */
	int i=0;
	int watchDog=10;

//	cout<<"Fadeev :"<<endl;

	while(watchDog&&!zero(Bi)) {
		++i;
		Ai=A*Bi;
		qi=Ai.trace()/i;

		Bi1=Bi;
		Bi=Ai-(qi*Matrice::Id);

//		cout	<<" * A"<<i<<" = "<<endl<<Ai
//			<<" * q"<<i<<" = "<<qi<<endl
//			<<" * B"<<i<<" = "<<endl<<Bi
//			<<" * zero(B"<<i<<") ? "<<zero(Bi)
//			<<endl
//			<<endl;
		--watchDog;
	}
	if(!watchDog) {
		throw(new exception());
	}

//	cout<<"determinant==qn ? "<<Ai.determinant()<<"=="<<qi<<endl;

	Ainv=(1./qi)*Bi1;

//	cout<<"*** Inv ="<<endl<<Ainv<<endl;
}







/* retourne l'inverse de la matrice */
Matrice Matrice::inverse() const {
	Matrice ret;
	fadeev(*this,ret);
	return ret;
}

/* retourne la transposition de la matrice */
Matrice Matrice::transpose() const {
	Matrice ret(	(*this)(0,0),(*this)(0,1),(*this)(0,2),
			(*this)(1,0),(*this)(1,1),(*this)(1,2),
			(*this)(2,0),(*this)(2,1),(*this)(2,2));
	return ret;
}

/* retourne la trace de la matrice */
Reel Matrice::trace() const {
	return (*this)(0,0)+(*this)(1,1)+(*this)(2,2);
}

/* donne le déterminant de la matrice */
Reel Matrice::determinant() const {
	const Reel&a=(*this)(0,0);
	const Reel&b=(*this)(0,1);
	const Reel&c=(*this)(0,2);
	const Reel&d=(*this)(1,0);
	const Reel&e=(*this)(1,1);
	const Reel&f=(*this)(1,2);
	const Reel&g=(*this)(2,0);
	const Reel&h=(*this)(2,1);
	const Reel&i=(*this)(2,2);

	return a*e*i + b*f*g + c*d*h - a*f*h - b*d*i - c*e*g;
}



const class Matrice Matrice::Id;



class Matrice operator+(const Matrice&A,const Matrice&B) {
	return Matrice(
		A(0,0)+B(0,0), A(1,0)+B(1,0), A(2,0)+B(2,0),
		A(0,1)+B(0,1), A(1,1)+B(1,1), A(2,1)+B(2,1),
		A(0,2)+B(0,2), A(1,2)+B(1,2), A(2,2)+B(2,2));
}

class Matrice operator-(const Matrice&A,const Matrice&B) {
	return Matrice(
		A(0,0)-B(0,0), A(1,0)-B(1,0), A(2,0)-B(2,0),
		A(0,1)-B(0,1), A(1,1)-B(1,1), A(2,1)-B(2,1),
		A(0,2)-B(0,2), A(1,2)-B(1,2), A(2,2)-B(2,2));
}


class Matrice operator*(const Matrice&A,const Matrice&B) {
	Matrice ret(0,0,0,0,0,0,0,0,0);
	for(int i=0;i<3;i++) {	/* parcours lignes */
		for(int j=0;j<3;j++) {	/* parcours colonnes */
			for(int k=0;k<3;k++) {	/* parcours en croix */
				ret(i,j)+=A(i,k)*B(k,j);
			}
		}
	}
	return ret;
}

class Matrice operator*(Reel k,const Matrice&m) {
	Matrice ret(	k*m(0,0),k*m(1,0),k*m(2,0),
			k*m(0,1),k*m(1,1),k*m(2,1),
			k*m(0,2),k*m(1,2),k*m(2,2));
	return ret;
}

//class Vecteur operator*(const Matrice&m,const Vecteur&v) {
//	Vecteur ret(0,0,0);
//	for(int i=0;i<3;i++) {
//		for(int k=0;k<3;k++) {
//			ret(i)+=v(k)*m(i,k);
//		}
//	}
//	return ret;
//}
//
class Vecteur operator*(const Matrice&m,const Position&p) {
	Vecteur v(p.x,p.y,1);
	Vecteur ret(0,0,0);
	for(int i=0;i<3;i++) {
		for(int k=0;k<3;k++) {
			ret(i)+=v(k)*m(i,k);
		}
	}
	return ret;
}

#define _epsilon_ 0.000001
bool zero(const Reel d) { return d>(-_epsilon_)&&d<_epsilon_; }

bool zero(const Matrice&m) {
	for(int i=0;i<3;i++) {
		for(int j=0;j<3;j++) {
			if(!zero(m(i,j))) {
				return false;
			}
		}
	}
	return true;
}

std::ostream& operator<<(std::ostream&o,const class Matrice&m) {
	for(int i=0;i<3;i++) {
		for(int j=0;j<3;j++) {
			o<<'\t'<<m(i,j);
		}
		o<<endl;
	}
	return o;
}
